#include "fire_control.h"

#include <string.h>
#include <math.h>
#include <stdlib.h>


extern float predict_time;
extern float distance_xy;

uint8_t shoot_allow;

#define SHOOT_DELAY 0.25f
#define CHOOSE_SPEED_VYAW 2.5f

#define PI (3.14159265358979323846f)
#define sign(_n)	(_n>=0?1.f:-1.f)

float theta=0.35f;
float armor_theta_diff = 0.f;
float  vyaw_bound = VYAW_BOUND;
float current_selected_armor_x = 0.f, current_selected_armor_y = 0.f, center_armor_x = 0.f, center_armor_y = 0.f;
float aim_spin_status = 0 ,shoot_delay = SHOOT_DELAY, choose_speed_vyaw = CHOOSE_SPEED_VYAW;

float _dt_f= 0.f, aim_err_allowance_SLOWSPEED = 0.1f, aim_err_allowance_HIGHSPEED = 0.24f;
uint8_t allow_fire_ang_delta = 80;

static inline int16_t get_delta_ang(int16_t ang1, int16_t ang2){
	return (ang1 - ang2 + ANGLE_RANGE*3/2) % ANGLE_RANGE- ANGLE_RANGE/2;
}

static inline float get_delta_ang_pi(float ang1, float ang2){
	float delta_previous = ang1 - ang2;
	if(delta_previous > PI) delta_previous -= 2*PI;
	else if(delta_previous < -PI) delta_previous += 2*PI;
	return delta_previous;
}

void select_amor(vision_ctrl_t* vision_ctrl_data,float* est_x,float* est_y,float* est_z)
{
    
		float yaw = vision_ctrl_data->yaw;
		float r1 = vision_ctrl_data->r1;
		float r2 = vision_ctrl_data->r2;
		float xc = vision_ctrl_data->x;
		float yc = vision_ctrl_data->y;
		float zc = vision_ctrl_data->z;
		float vx = vision_ctrl_data->vx;
		float vy = vision_ctrl_data->vy;
		float vz = vision_ctrl_data->vz;
		float vyaw = vision_ctrl_data->v_yaw;
		float dz = vision_ctrl_data->dz;
		float armor_num = vision_ctrl_data->armor_num;		
		// Prediction
    
		xc = xc + predict_time * vx;
		yc = yc + predict_time * vy;
		zc = zc + predict_time * vz;
		float yaw_predict_time = predict_time + (fabs(vyaw) > vyaw_bound ? shoot_delay : 0.f);
		yaw = yaw + yaw_predict_time * vyaw;

		float center_theta = atan2(yc, xc); //vision_request.yaw;
		uint8_t use_1 = 1;	
		uint8_t armor_number = armor_num; 
		float diff_angle = 2*PI/armor_num;
		float add_theta = theta * -sign(vyaw);

		if(fabs(vyaw) > choose_speed_vyaw)
		{
			for (size_t i = 0; i < armor_number; i++)
			{
				float armor_yaw = yaw + i * diff_angle;
				float yaw_diff = get_delta_ang_pi(armor_yaw, center_theta);
				if ((-PI/4 + 0.08f +add_theta) < yaw_diff && yaw_diff < (PI/4 - 0.08f +add_theta))
				{		
					armor_theta_diff = yaw_diff;

					// Only 4 armors has 2 radius and height
					float r = r1;
					*est_z = zc;
					if (armor_num == 4) {
						r = use_1 ? r1 : r2;
						*est_z = use_1 ? zc : (zc+dz);
					}

					// Robot state to armor
					current_selected_armor_x = xc - r * cos(armor_yaw);
					current_selected_armor_y = yc - r * sin(armor_yaw);

					center_armor_x = xc - r * cos(center_theta);
					center_armor_y = yc - r * sin(center_theta);
					
					if(fabs(vyaw) > vyaw_bound){
						aim_spin_status = HIGH_SPEED;			
						*est_x = center_armor_x;
						*est_y = center_armor_y;
					}else{
						aim_spin_status = SLOW_SPEED;
						*est_x = current_selected_armor_x;
						*est_y = current_selected_armor_y;
					}
					break;
				}
				use_1 = !use_1;
			}
		}else{
			*est_x = xc- r1 * cos(yaw);
			*est_y = yc- r1 * sin(yaw);
			*est_z = zc;
		}
}
int test;
void shoot_allow_control(int16_t yaw_ang_ref,float yaw)
{
        if (distance_xy != 0)
        {
            allow_fire_ang_delta = aim_err_allowance_SLOWSPEED / PI * 4096.0f / distance_xy;
        }
        if (aim_spin_status == HIGH_SPEED)
        {
            shoot_allow = armor_theta_diff < (aim_err_allowance_HIGHSPEED / distance_xy);
        }
        else
        {   
					 test=abs(get_delta_ang(yaw_ang_ref, yaw));
            if (abs(get_delta_ang(yaw_ang_ref, yaw)) < (uint8_t)allow_fire_ang_delta)
            {
                shoot_allow = 1;
            }
            else
            {
                shoot_allow = 0;
            }
        }
        if (fabs(vision_ctrl_data.v_yaw) > 9.5f)
            shoot_allow = 1;
        if (vision_ctrl_data.target_found == 0)
            shoot_allow = 0;
}
